/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
// MCL.cpp : Defines the entry point for the console application.
//
//Monte Carlo Localization (MCL)
//
//Particle based Localisation algorithm that takes laserscans and map as inputs 
//and gives place and orientation of robot as outputs
//

// Init::TPoseParticleDistribution getInitialDistribution(TPose initPos,

//Result is sent to WoPa as a locate() command
//
//
/******************************************************/

#include <math.h>
#include <stdlib.h>
#include "mcl_interface.h"
//#include "settings.h"

/**
* A help function to tell if it is ok to use the MCL
*/
bool mcl_isOk(TMCLData &MCL){
    
    if(!MCL.isMapLoaded){
        fprintf(stderr,"MCL:: Map not loaded!\n");
        return false;
    } 
    
    return (MCL.isMapLoaded && MCL.isInitialized);
}
/**
* Loads the map for the MCL from the file 
*/ 
void mcl_loadMapFromFile(TMCLData &MCL, const char *filename){
    
    MCL.map = readMapDataFromFile(filename,(float)(1.0/1.0));
	MCL.isMapLoaded = true;
#ifdef JB_MCL_DEBUG	
    fprintf(stderr,"SIZE OF GLOBAL MAP %d\n",MCL.map.NumOfLines);
/******************************************************************
	for(int i=0;i<10;i++)
		fprintf(stderr,"%.1f %.1f %.1f %.1f\n",MCL.map.Lines[i].x1,
											   MCL.map.Lines[i].y1,
											   MCL.map.Lines[i].x2,
											   MCL.map.Lines[i].y2);
********************************************************************/
#endif
}

/**
* Loads the map from server command 
*/
void mcl_loadMapFromServer(TMCLData &MCL, char *buf){
  MCL.isMapLoaded = handleJ2B2ServerLinevectorString(buf, MCL.map);
  if (!MCL.isMapLoaded)
    fprintf(stderr,"mcl_loadMapFromServer:: Unable to read the map from string '%s'\n",buf);
}

//*******************************************************************************************
void initMCL(TMCLData &MCL){
	//int i;	
	//MCL.map = readMapDataFromFile("autMapHandled.txt",(float)(1.0/1.0));
	//fprintf(stderr,"SIZE OF GLOBAL MAP %d\n",MCL.map.NumOfLines);
	//for(i=0;i<10;i++)
	//	fprintf(stderr,"%.1f %.1f %.1f %.1f\n",MCL.map.Lines[i].x1,
	//										   MCL.map.Lines[i].y1,
	//										   MCL.map.Lines[i].x2,
	//										   MCL.map.Lines[i].y2);
#ifdef MCL_SAVE_TO_FILE	
	FILE *f;
	//Sleep(5000);
	f= fopen("part.txt","wt");
	fclose(f);
	f = fopen("path.txt","wt");
	fclose(f);
#endif
    
    MCL.Qini[0] = (float)1.0;
	MCL.Qini[1] = (float)1.0;
	MCL.Qini[2] = (float) ( 10.0*PI/180.0);
	MCL.isInitialized = false;
	MCL.scan = (float *) malloc(2 * sizeof(float) * (NUMBER_OF_MEASUREMENTS+2)); 
	MCL.isMapLoaded = false;

	MCL.Q[0] = (float)0.02;
	MCL.Q[1] = (float)0.02;
	MCL.Q[2] = (float)(1.0*PI/180.0);

	MCL.numOfSIRupdates=0;
	MCL.meanPos.x=0;
	MCL.meanPos.y=0;
	MCL.meanPos.a=0;
	MCL.meanPos.time=0;
	MCL.timesBadLik=0;

	MCL.PPD.NumOfParticles = 0;
	
	MCL.odoOld.x = 0;
	MCL.odoOld.y = 0;
	MCL.odoOld.a = 0;

	mcl_loadMapFromFile(MCL,DEFAULT_MCL_MAP_FILE);
	
	//MCL.PPD = estimateInitialPosition(initPos,numberOfParticles,v,Q,cMap,scan);
}

/**
* Initialises the particle distribution to some given initial pose
*/
void mcl_initializeDistribution(TMCLData &MCL, TPose initialPose, int numberOfParticles){
    
        MCL.PPD = getInitialDistribution(initialPose,MCL.Qini,numberOfParticles);
        MCL.isInitialized = true;
    
}
//***********************************************************************************
void scan2MCLscan(float *r, float *mclR){
	int i,j=0;

#ifdef MCL_SAVE_TO_FILE	
	FILE *f;
	f = fopen("meas.txt","wt");
#endif
	
	for(i=0; i < 361; i+= (int)(360/(NUMBER_OF_MEASUREMENTS))){
		
		mclR[2*j]   = r[i] * cos(((float)i * 0.5 * PI / 180.0) - PI/2.0);
		mclR[2*j+1] = r[i] * sin(((float)i * 0.5 * PI / 180.0) - PI/2.0);

#ifdef MCL_SAVE_TO_FILE	
		fprintf(f,"%.3f %.3f\n",mclR[2*j],mclR[2*j+1]);
#endif		
		j++;

		if(i>360 || j >= NUMBER_OF_MEASUREMENTS) {
			//fprintf(stderr,"Warning, scan2MCLscan, i=%d j=%d\n",i,j);
			break;
		}
	}
#ifdef MCL_SAVE_TO_FILE	
	fclose(f);
#endif
}
//************************************************************************************
/**
* The MCL Calculation
* Inputs are current scan, The pose where the scan has been taken and the MCL datastructure
* @param *scan      The range scan
* @param current    Pose where the scan has been taken according to the odometry
* @param &MCL       Reference to the MCL data structure
*/
void calculateMCL(float *scan, TPose current, TMCLData &MCL){
	TLineVector lmap;
	float dx0,dy0, dl,beta,alpha;
	TPose old;
#ifdef MCL_SAVE_TO_FILE	
	int i;
	FILE *f;
#endif
    
#ifdef JB_MCL_DEBUG
    old = MCL.odoOld;
    fprintf(stderr,"Inputs:: NEW --> x='%.2f' y=''%.2f a='%.2f'\n",current.x,current.y,current.a); 
    fprintf(stderr,"Inputs:: OLD --> x='%.2f' y=''%.2f a='%.2f'\n",old.x,old.y,old.a);
    fprintf(stderr,"Map size %d\n",MCL.map.NumOfLines);

//fprintf(stderr,"Scan -->\n");
    //for(i=0;i<361;i++){
    //    fprintf(stderr,"%.1f ",scan[i]);
    //}
#endif

	
    if(!mcl_isOk(MCL)){
        fprintf(stderr,"MCL not fully initialized\n");
        return;
    }
    
	old = MCL.odoOld;
	// Calculate the reduced scan 
	scan2MCLscan(scan,MCL.scan);
	
	// Movement according to the odometry
	// Calculate The relative motion between scans
	// The scan are transformed to the "global" (from odometry point of view)
	// Frame of reference
	dx0 = current.x-old.x;
	dy0 = current.y-old.y;
	dl = sqrt(dx0*dx0 + dy0*dy0);
	
	beta = atan2(dy0,dx0);
	if(beta<0) beta += (float)2.0f*(float)PI; // transform the angle to 0...2*PI  
	alpha = beta - old.a; 
	
	MCL.v[0] = dl * cos(alpha); 
	MCL.v[1] = dl * sin(alpha);
	MCL.v[2] = (current.a-old.a);
	
	//fprintf(stderr,"MCLPos: %.2f %.2f %.2f  :",MCL.meanPos.x, MCL.meanPos.y, MCL.meanPos.a);
	
	lmap = getLocalMap(MCL.meanPos,MCL.map);
	if(lmap.size <= 0){
	  fprintf(stderr,"calculateMCL:: The linemap does not have any lines - Cannot update\n");
	  return;
	}
	//lmap = MCL.map;
	//fprintf(stderr,"Number of lines in local map = %d in total %d\n",lmap.NumOfLines,
	//															MCL.map.NumOfLines);
	
		if(MCL.numOfSIRupdates > 10){
			
			MCL.PPD = MCLBasicStep(MCL.PPD,MCL.v,MCL.Q,lmap,MCL.scan);
			MCL.numOfSIRupdates=0;
		}else{
			MCL.PPD = MCLBasicStepNoSIR(MCL.PPD,MCL.v,MCL.Q,lmap,MCL.scan);
			MCL.numOfSIRupdates++;
		}
		
#ifdef MCL_SAVE_TO_FILE	
		f= fopen("part.txt","at");
		fprintf(f,"%.2f %.2f %.2f ",current.x,current.y,current.a);
		for(i=0;i<MCL.PPD.NumOfParticles;i++){
			fprintf(f,"%.2f %.2f %.2f ",MCL.PPD.Particles[i].x, MCL.PPD.Particles[i].y, MCL.PPD.Particles[i].a);
		}
		fprintf(f,"\n");
		fclose(f);
#endif
		MCL.meanPos = calculateMeanPosition(MCL.PPD);
		MCL.meanPos.time = current.time;
		MCL.varPos = calculateParticleVariances(MCL.PPD);
		
		if(MCL.PPD.Lik <1.5 ){
			MCL.timesBadLik ++;
#ifdef MCL_DEBUG
			debug(DEBUGS,"LIK:%f \n",MCL.PPD.Lik);
#endif
		}
		else{
			MCL.timesBadLik=0;
		}
		
		
		
		if(MCL.timesBadLik >20){
			MCL.timesBadLik=0;
			MCL.PPD = estimateInitialPosition(MCL.meanPos,
				NUM_OF_PARTICLES,MCL.v,
				MCL.Qini,lmap,MCL.scan);
			
		}
		
		MCL.odoOld = current;
		freeLineVector(lmap);
		
#ifdef JB_MCL_DEBUG
		fprintf(stderr,"X:%.1f Y:%.1f A:%.1f :: Vx:%.1f Vy:%.1f Va:%.1f :: Lik:%.1f dl:%.2f\n",
			MCL.meanPos.x,MCL.meanPos.y,MCL.meanPos.a,
			sqrt(MCL.varPos.x),sqrt(MCL.varPos.y),sqrt(MCL.varPos.a),
			MCL.PPD.Lik, 
			sqrt(MCL.v[0]*MCL.v[0]+MCL.v[1]*MCL.v[1]));
#endif
}
	//****************************************************************
	
	
void freeMCL(TMCLData &MCL){
	free(MCL.scan);

}
